/*
 * Copyright (c) 2006-2021, Halo team
 *
 * SPDX-License-Identifier: GPLv3
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-09-11     Eywen        the first version
 */

#include "drivers/drv_smart_step.h"
#include "common.h"
#include <math.h>

#define DBG_TAG "drv_smart_step"
#define DBG_LVL DBG_LOG

static bool smart_step_init(smart_step_t *device, int pin_pwm, int pin_en, int pin_dir,
                            uint32_t timer_freq, smart_step_id_t id);
static void smart_step_pause(smart_step_t *device);
static void smart_step_clear(smart_step_t *device);
static void smart_step_start(smart_step_t *device);
inline static void smart_step_status_set(smart_step_t *device, smart_step_status_t st);
static bool smart_step_task_is_full(smart_step_t *device);
static bool smart_step_add_task(smart_step_t *device, smart_step_task_t *task);
static void smart_step_rotation_set(smart_step_t *device, smart_step_rotation_t rotation);
static void smart_step_enable(smart_step_t *device, bool en);
static void smart_step_forward_level_set(smart_step_t *device, bool level);
static void smart_step_cur_pulse_loc_clear(smart_step_t *device);
static void smart_step_timer_schdule(smart_step_t *device);
static bool smart_step_is_motion(smart_step_t *step);

/* 清除掉任务队列全部的任务 */
void smart_step_clear(smart_step_t *device)
{
    ASSERT_NULL(device);

    smart_step_status_set(device, smart_step_st_idle);
    ringbuff_clear(device->pulse_list);
}

/* 设置电机控制标志，让电机开始从tasklist中读取任务并运行 */
inline void smart_step_start(smart_step_t *device)
{
    ASSERT_NULL(device);

    smart_step_status_set(device, smart_step_st_running);
}

/* 暂停电机输出控制脉冲 */
void smart_step_pause(smart_step_t *device)
{
    ASSERT_NULL(device);

    smart_step_status_set(device, smart_step_st_paused);
}

/* 设置当前步进电机状态标志 */
inline static void smart_step_status_set(smart_step_t *device, smart_step_status_t st)
{
    ASSERT_NULL(device);

    device->status = st;
}

/* 使用定时器定时调度这个函数 */
#if (defined SOC_AB32VG1)
RT_SECTION(".irq.timer")
#endif
void smart_step_timer_schdule(smart_step_t *device)
{
    ASSERT_NULL(device);
    smart_step_task_t *cur_task;
    smart_step_cnt_t *counter;

    /* 当前不在运行状态,则不需要再额外计算了*/
    if(device->status != smart_step_st_running){
        return;
    }
    cur_task = &(device->cur_task);
    counter = &(device->counter);
    /* 计数器跑完了并且脉冲计数也减完毕了,因此需要载入新的任务 */
    if(counter->cnt == 0){
        if(cur_task->pulse == 0){
            /* 队列为空了,因此不需要再处理 */
            if(ringbuff_is_empty(device->pulse_list) == true){
                smart_step_status_set(device, smart_step_st_idle);
                //debug_info("task list empty...");
                return;
            }
            else{

                /* 当前任务完毕,载入新的任务 */
                float freq;

                for(;;){
                    /* 加入任务到任务队列 */
                    ringbuff_pop(device->pulse_list, cur_task);
                    //debug_info("cur_task->pulse: %x", cur_task->pulse);
                    if((cur_task->pulse == (uint32_t)0)){
                        if(true == ringbuff_is_empty(device->pulse_list)){
                            smart_step_status_set(device, smart_step_st_idle);
                            //debug_error("cur_task->pulse: %x", cur_task->pulse);
                            return;
                        }
                        continue;
                    }
                    /* 防止频率过慢停留太久 */
                    if(fabsf(cur_task->freq) < 100){
                        if(cur_task->freq > 0){
                            cur_task->freq = 100;
                        }
                        else{
                            cur_task->freq = -100;
                        }
                    }
                    //debug_info("%d %u", (int)cur_task->freq, cur_task->pulse);
                    break;
                }

                /* 根据频率正负设置方向 */
                if(cur_task->freq < 0){
                    /* 频率小于0,反向转 */
                    smart_step_rotation_set(device, smart_step_rotation_reverse);
                    //debug_info("smart_step_rotation_set");
                    freq = -cur_task->freq;
                }
                else{
                    /* 频率大于0.正向转动 */
                    smart_step_rotation_set(device, smart_step_rotation_forward);
                    freq = cur_task->freq;
                }
                /* 软件计数器重载和计数值设置 */
                counter->reload = ((uint32_t)(device->timer_freq/freq) >> 1);
                counter->cnt = counter->reload;
                cur_task->pulse = (cur_task->pulse<<1) - 1;
                //debug_info("pop task: %d %x %u\r\n", (int)cur_task->freq, (uint32_t)cur_task->pulse, counter->reload);
            }
        }
        else{
            //rt_pin_write(device->pin_pwm, !(rt_pin_read(device->pin_pwm)));
            cur_task->pulse--;
            counter->cnt = counter->reload;
            //rt_pin_write(device->pin_pwm, !(rt_pin_read(device->pin_pwm)));
            //device->cur_pulse_loc += ((cur_task->freq > 0) ? 1: -1);
        }
    }
    counter->cnt--;

    if(counter->cnt == 0){
        rt_pin_write(device->pin_pwm, !(rt_pin_read(device->pin_pwm)));
        device->cur_pulse_loc += ((cur_task->freq > 0) ? 1: -1);
    }

    //debug_info("counter->cnt: %u reload: %u pulse: %u", counter->cnt, counter->reload, cur_task->pulse);
//    /* 占空比50% */
//    if(counter->cnt == 0){
//        //rt_pin_turn(device->pin_pwm);
//
//    }
}

/* 返回任务队列是否满 */
bool smart_step_task_is_full(smart_step_t *device)
{
    ASSERT_NULL(device);

    return ringbuff_is_full(device->pulse_list);
}


bool smart_step_task_is_empty(smart_step_t *device)
{
    return ringbuff_is_empty(device->pulse_list);
}

/* 设置步进电机转动方向 */
void smart_step_rotation_set(smart_step_t *device, smart_step_rotation_t rotation)
{
    if(rotation == smart_step_rotation_forward){
        rt_pin_write(device->pin_dir, device->forward_rotation_level);
    }
    else{
        rt_pin_write(device->pin_dir, !(device->forward_rotation_level));
    }
}

static bool smart_step_is_motion(smart_step_t *step)
{
    return (step->status == smart_step_st_running);
}


/* 初始化步进电机控制句柄 */
bool smart_step_init(smart_step_t *device, int pin_pwm, int pin_en,int pin_dir,
                     uint32_t timer_freq, smart_step_id_t id)
{
    LOG_D("smart pwm init");

    ASSERT_NULL(device);
    ASSERT_NULL(device->ops);

    device->pin_pwm = pin_pwm;
    device->pin_en = pin_en;
    device->pin_dir = pin_dir;

    smart_step_status_set(device, smart_step_st_idle);
    device->counter.cnt = 0;
    device->counter.reload = 0;
    device->timer_freq = timer_freq;
    device->id = id;

    /* 记录电机转动的脉冲位置,用来获得当前位置,初始值不可靠,因此需要启动后定位,
     * 这个是绝对位置,即相对限位开关的位置 */
    device->cur_pulse_loc = 0;

    device->cur_task.freq = 0;
    device->cur_task.pulse = 0;

    /* 初始化引脚 */
    rt_pin_mode(device->pin_pwm, PIN_MODE_OUTPUT);
    rt_pin_write(device->pin_pwm, PIN_LOW);
    /* 初始化电机使能引脚和方向引脚 */
    rt_pin_mode(device->pin_en, PIN_MODE_OUTPUT);
    rt_pin_write(device->pin_en, PIN_LOW);
    rt_pin_mode(device->pin_dir, PIN_MODE_OUTPUT);
    rt_pin_write(device->pin_dir, PIN_LOW);

    return true;
}

/* 使能/去能步进电机控制器 */
void smart_step_enable(smart_step_t *device, bool en)
{
    ASSERT_NULL(device);

    if(en == true){
        rt_pin_write(device->pin_en, device->enable_level);
    }
    else{
        rt_pin_write(device->pin_en, !(device->enable_level));
    }
}

/* 设置步进电机正传方向的电平标志 */
void smart_step_forward_level_set(smart_step_t *device, bool level)
{
    ASSERT_NULL(device);
    device->forward_rotation_level = level;
}

/* 当前脉冲位置标志清0，可用于触发限位开关的控制 */
inline void smart_step_cur_pulse_loc_clear(smart_step_t *device)
{
    ASSERT_NULL(device);

    device->cur_pulse_loc = 0;
}

/* 将新任务放入任务队列 */
bool smart_step_add_task(smart_step_t *device, smart_step_task_t *task)
{
    ASSERT_NULL(device);
    if(task->pulse == 0){
        return true;
    }
    //debug_info("add task step: freq: %d pulse: %u", (int)task->freq, task->pulse);
    return ringbuff_push(device->pulse_list, task);
}

/* 返回当前脉冲位置标志的值 */
uint32_t smart_step_pulse_get(smart_step_t *device)
{
    ASSERT_NULL(device);
    return device->cur_pulse_loc;
}

static smart_step_status_t smart_step_status_get(smart_step_t *device)
{
    ASSERT_NULL(device);
    return device->status;
}

/* 驱动操作接口 */
static smart_step_ops_t s_smart_step_ops = {
    .init = smart_step_init,
    .clear = smart_step_clear,
    .start = smart_step_start,
    .pause = smart_step_pause,
    .timer_schdule = smart_step_timer_schdule,
    .task_is_full = smart_step_task_is_full,
    .task_is_empty = smart_step_task_is_empty,
    .add_task = smart_step_add_task,
    .enable = smart_step_enable,
    .release = NULL,
    .pulse = smart_step_pulse_get,
    .forward_level_set = smart_step_forward_level_set,
    .reset_cur_pulse_loc = smart_step_cur_pulse_loc_clear,
    .status = smart_step_status_get,
    .is_motion = smart_step_is_motion,
};

/* 驱动对象实体 */
static smart_step_t s_smart_step_drv_list[smart_step_max] = {
    {
        .id = smart_step_0,
        .pin_pwm = -1,
        .ops = &s_smart_step_ops,
    },
    {
        .id = smart_step_1,
        .pin_pwm = -1,
        .ops = &s_smart_step_ops,
    },
    {
        .id = smart_step_2,
        .pin_pwm = -1,
        .ops = &s_smart_step_ops,
    },
    {
        .id = smart_step_3,
        .pin_pwm = -1,
        .ops = &s_smart_step_ops,
    },
};

/* 注册操作接口,在创建动态smart_step_t对象后,必须先调用本函数才能使用ops接口 */
bool smart_step_register(smart_step_t *smart_step)
{
    debug_info("smart step register");

    ASSERT_NULL(smart_step);

    MEM_MEMSET(smart_step, 0, sizeof(smart_step_t));

    /* 创建ringbuff标识空间 */
    smart_step->pulse_list = MEM_MALLOC(sizeof(ringbuff_t));
    if(smart_step->pulse_list == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }

    int ringbuff_size;
    void *ptr;
    ringbuff_size = SMART_STEP_TASK_BUFF_LEN*sizeof(smart_step_task_t);
    /* 申请存储ringbuff内容的空间 */
    ptr = MEM_MALLOC(ringbuff_size);
    if(ptr == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }

    /* 初始化ringbuff，绑定空间与ringbuff变量 */
    ringbuff_init(smart_step->pulse_list, ptr, ringbuff_size, sizeof(smart_step_task_t));

    smart_step->ops = &s_smart_step_ops;

    debug_info("smart step register success");
    return true;
err:
    debug_error("there is not enough memory");
    if(smart_step->pulse_list->addr != NULL){
            MEM_FREE(smart_step->pulse_list->addr);
            smart_step->pulse_list->addr = NULL;
    }

    if(smart_step->pulse_list != NULL){
        MEM_FREE(smart_step->pulse_list);
        smart_step->pulse_list = NULL;
    }

    debug_error("smart step drv register fail");
    return false;
}

/* 通过步进电机的id查找步进电机的控制句柄 */
smart_step_t *smart_step_find_by_id(smart_step_id_t id)
{
    for(smart_step_id_t idx = smart_step_0; idx < smart_step_max; idx++){
        if(s_smart_step_drv_list[idx].id == id){
            return &(s_smart_step_drv_list[idx]);
        }
    }
    return NULL;
}

